Calibrating a Tightly-Coupled GNSS/MU Integration Filter

ABSTRACT

Embodiments of the invention provide methods of calibrating a blending filter based on extended Kalman filter (EKF), which optimally integrates the IMU navigation data with all other satellite measurements (tightly-coupled integration filter). In one embodiment a coordinate transformation matrix using a latest position fix is created. The state variables (for user velocity) are transformed to a local navigation coordinate. The state variables of said integration filter is estimated. A blended calibrated position fix is the output of the method.

CROSS-REFERENCE TO RELATED APPLICATIONS

This application is a Divisional of and claims priority to U.S.application Ser. No. 12/568,846, filed on Oct. 21, 2009, which is aContinuation-In-Part and claims priority under 35 U.S.C. §120 to U.S.application Ser. No. 12/568,084, filed on Sep. 28, 2009 and claimspriority under 35 U.S.C. §119(e) to U.S. Provisional Application No.61/107,090, filed on Oct. 21, 2008. This application is also related toU.S. Provisional Application No. 61/099,631, filed on Sep. 24, 2008,Non-Provisional application Ser. No. 12/565,927, filed Sep. 24, 2009, TIDocket Number TI-67096, entitled DETECTING LACK OF MOVEMENT TO AID GNSSRECEIVERS. This application is related to co-pending U.S. patentapplication Ser. No. 12/394,404, filed on Feb. 27, 2009, entitled METHODAND SYSTEM FOR GNSS COEXISTENCE. All of said applications incorporatedherein by reference.

BACKGROUND

Embodiments of the invention are directed, in general, to navigationsystems and, more specifically, to global navigation satellite system(GNSS) and inertial measurement unit (IMU) integration.

Any satellite-based navigation system suffers significant performancedegradation when satellite signal is blocked, attenuated and/orreflected (multipath), for example, indoor and in urban canyons. As MEMS(micro-electro-mechanical systems) technologies advance, it becomes moreinteresting to integrate sensor-based inertial navigation system (INS)solutions into GNSS receivers, in pedestrian applications as well as invehicle applications.

As GNSS receivers become more common, users continue to expect improvedperformance in increasingly difficult scenarios. GNSS receivers mayprocess signals from one or more satellites from one or more differentsatellite systems. Currently existing satellite systems include globalpositioning system (GPS), and the Russian global navigation satellitesystem (Russian:

, abbreviation of

,

,

,

tr.: GLObal'naya NAvigatsionnaya Sputnikovaya Sistema; “GLObalNAvigation Satellite System” (GLONASS). Systems expected to becomeoperational in the near future include Galileo, quasi-zenith satellitesystem (QZSS), and the Chinese system Beidou. For many years, inertialnavigation systems have been used in high-cost applications such asairplanes to aid GNSS receivers in difficult environments. One examplethat uses inertial sensors to allow improved carrier-phase tracking maybe found in A. Soloviev, S. Gunawardena, and F. van Graas, “Deeplyintegrated GPS/Low-cost IMU for low CNR signal processing: conceptdescription and in-flight demonstration,” Journal of the Institute ofNavigation, vol. 55, No. 1, Spring 2008; incorporated herein byreference. The recent trend is to try to integrate a GNSS receiver withlow-cost inertial sensors to improve performance when many or allsatellite signals are severely attenuated or otherwise unavailable. Thehigh-cost and low-cost applications for these inertial sensors are verydifferent because of the quality and kinds of sensors that areavailable. The problem is to find ways that inexpensive or low-costsensors can provide useful information to the GNSS receiver.

The inertial measurement unit (IMU) may include any of the following:accelerometers, magnetometers, and/or gyroscopes. And the IMU providesindependent navigation information regardless of the GNSS signalcondition. In many commercial applications, low-accuracy inertialsensors are used because of cost constraint. This invention providesmethods for estimating and compensating the navigation error due tousing low-quality IMU, while integrating the IMU-based measurements withthe GNSS-based measurements.

Sources of Dead Reckoning Errors

For pedestrian navigation, pedestrian dead reckoning (PDR) technique maybe implemented because it suffices to use relatively low-accuracysensors. The PDR is usually based on step detection, step lengthestimation, and heading determination. Considered are the followingtypes of DR errors.

-   -   Speed bias/error: Any inaccuracy in step length estimation        results in speed error/bias in the DR measurement.    -   Heading bias/error: Heading error due to soft-iron effect (local        magnetic disturbance) is generally difficult to estimate and        compensate since it is usually location-dependent. However,        relatively large heading bias due to different attitude of IMU        (from assumed one) can be estimated and compensated. For        example, mounting IMU on right-side of waist will have 90        degrees of heading bias compared with the case of mounting the        IMU on back waist.

Similarly, in vehicular applications, speed and heading biases arecommonly observed in the INS measurement. And they are main sources ofthe error in the final user position and velocity estimate.

Therefore, there is a need for a tightly-coupled blending filters withcalibration features built-in to track the speed and heading biases.

SUMMARY

In light of the foregoing background, embodiments of the inventionprovide a blending filter based on extended Kalman filter (EKF), whichoptimally integrates the IMU navigation data with all other satellitemeasurements (tightly-coupled integration filter). Two more states inthe EKF for estimating/compensating the speed bias and the heading biasin the INS measurement are added.

The integration filter has no feedback loop for INS calibration, and canestimate/compensate the navigation error in the INS measurement withinthe integration filter.

Therefore, the system and method of embodiments of the invention solvethe problems identified by prior techniques and provide additionaladvantages.

BRIEF DESCRIPTION OF THE DRAWINGS

Having thus described the invention in general terms, reference will nowbe made to the accompanying drawings, which are not necessarily drawn toscale, and wherein:

FIG. 1 is a block diagram of a global positioning system (GPS) receiverknown in the art.

FIG. 2 shows a tightly-coupled GNSS/IMU integration (top-level) inaccordance with an embodiment of the invention.

FIG. 3 is a flowchart illustrative of a methods of calibration inaccordance with an embodiments of the invention.

DETAILED DESCRIPTION

The invention now will be described more fully hereinafter withreference to the accompanying drawings. This invention may, however, beembodied in many different forms and should not be construed as limitedto the embodiments set forth herein. Rather, these embodiments areprovided so that this disclosure will be thorough and complete, and willfully convey the scope of the invention to those skilled in the art. Oneskilled in the art may be able to use the various embodiments of theinvention.

FIG. 1 is a block diagram of a global positioning system (GPS) receiver10 known in the art. The GPS receiver 10 includes a GPS antenna 12, asignal processor 14, a navigation processor 16, a real time clock (RTC)18, a GPS time detector 20, a hot start memory 22, a data updateregulator 30 and a user interface 31. GPS signal sources 32A-D broadcastrespective GPS signals 34A-D. The GPS signal sources 32A-D are normallyGPS satellites. However, pseudolites may also be used. For conveniencethe GPS signal sources 32A-D are referred to as GPS satellites 32 andthe GPS signals 34A-D are referred to as GPS signals 34 with theunderstanding that each of the GPS signals 34A-D is broadcast separatelywith separate GPS message data for each of the GPS signal sources 32A-D.A global navigation satellite system (GNSS) signal source and signal maybe used in place of the GPS signal sources 32 and GPS signals 34. Thereceiver is described in the context of processing GPS signals, but canbe used in the context of processing signals from any satellite system.

In order to more easily understand the embodiments of the invention, thestructural elements of the best mode of the invention are described interms of the functions that they perform to carry out the invention. Itis to be understood that these elements are implemented as hardwarecomponents and software instructions that are read by a microprocessorin a microprocessor system 35 or by digital signal processing hardwareto carry out the functions that are described.

The GPS antenna 12 converts the GPS signals 34 from an incoming airwaveform to conducted form and passes the conducted GPS signals to thesignal processor 14. The signal processor 14 includes a frequencydown-converter; and carrier, code and data bit signal recovery circuits.The frequency down-converter converts the conducted GPS signals to alower frequency and digitizes the lower frequency GPS signals to providedigital GPS signals. The signal recovery circuits operate on the digitalGPS signals to acquire and track the carrier, code and navigation databits for providing respective timing signals 38 and GPS data bit streams40 for each of the GPS satellites 32. Parallel processing of therespective digital GPS signals is preferred so that the timing signals38 and the data bit streams 40 are determined in parallel for severalGPS satellites 32, typically four or more. The timing signals 38generally include code phase, code chip timing, code cycle timing, databit timing, and Doppler tuning.

The timing signals 38 are passed to the navigation processor 16 and thedata bit streams 40 are passed to the GPS time detector 20 and the dataupdate regulator 30. The GPS time detector 20 uses GPS clock timeestimates 42 from the RTC 18 and the data bit streams 40 for determininga true GPS clock time 44 and passes the true GPS clock time 44 to thenavigation processor 16. The navigation processor 16 includes apseudorange calculator and a position detector using the timing signals38 and the GPS clock time 44 for determining pseudoranges between theGPS antenna 12 and the GPS satellites 32 and then using the pseudorangesfor determining a position fix. The navigation processor 16 passes theGPS clock time and position to the user interface 31.

The data update regulator 30 passes a specified collection 48 of databits of the GPS data bit streams 40 to a data chapter memory 50 withinthe GPS time detector 20 for updating a block of GPS message data in thechapter memory 50. The user interface 31 may include keys, a digitalinput/output capability and a display for enabling a user to operate theGPS receiver 10 and view results of the operation of the GPS receiver10. In general the user interface 31 is coupled through themicroprocessor system 35 to each of the other elements of the GPSreceiver 10.

The GPS receiver 10 also includes a standby mode regulator 52. Thestandby mode regulation 52 controls the GPS receiver 10 through controlsignals 54 to have an operation mode and a standby mode. The GPSreceiver 10 may be directed to enter the standby mode at any time fromthe user interface 31.

In the operation mode, the GPS receiver 10 acquires the GPS signals 34and determines a true GPS clock time 44; and uses the GPS clock time 44for determining a two or three dimensional position fix. If time only isrequired, the GPS receiver 10 returns to the standby mode withoutdetermining the position fix. During the standby mode, the GPS receiver10 reduces its power consumption and maintains standby data, includingits position, in the hot start memory 22 for a state of readiness. Thestandby data includes the last known GPS time and position of the GPSreceiver 10. Data for GPS ephemeris and almanac orbital parameters 56 isstored in the hot start memory 22 or the chapter memory 50.

When the GPS receiver 10 enters the operation mode after a time periodin the standby mode, the signal processor 14 uses the GPS clock timeestimates 42, the almanac or ephemeris parameters and the standby datafor quickly providing the signal timing signals 38 and the data bitstream 40. The navigation processor 16 uses the GPS clock time 44, thestored ephemeris parameters, and the timing signals 38 in order tocompute a first position fix for what is known as a hot start fast timeto first fix (TTFF). The microprocessor system 35 is interconnected forcontrolling the signal processor 14, navigation processor 16, real timeclock (RTC) 18, GPS time detector 20, hot start memory 22, data updateregulator 30, user interface 31, data chapter memory 50 and standby moderegulator 52. The functions of the signal processor 14, navigationprocessor 16, real time clock (RTC) 18, GPS time detector 20, hot startmemory 22, data update regulator 30, user interface 31, data chaptermemory 50 and standby mode regulator 52 are implemented by themicroprocessor 35 according to programmed software instructions on oneor more computer readable mediums or by digital signal processinghardware or by a combination.

FIG. 2 shows a top-level block diagram for tightly-coupled GNSS/IMUintegration. Analog front end (AFE) 210 converts analog data to digitaldata. The GNSS measurement engine 220 provides the blending EKFintegration filter 240 with the following 230: pseudorange measurement,delta range measurement for each satellite. The measurement engine canalso provide measurement noise variances for the pseudorangemeasurements and delta range measurements.

The inertial navigation system (INS) 270 or pedestrian dead reckoning(PDR) block calculates navigation information (position and/or velocity)using the inertial sensor outputs. The output of INS 270 or PDR may alsobe referred to as DR measurement (DR stands for dead reckoning). Theproposed GNSS/IMU integration filter in accordance with embodiments ofthe invention uses the user velocity data given by or derived from theDR measurement (output of the INS 270 or PDR block). The INS uservelocity data may be synchronized to the GNSS measurement samples.Although FIG. 2 shows the IMU 280 with accelerometers 285 andmagnetometers 287, this is just an example. The IMU 280 may have otherkinds of sensor combinations as well, including gyroscopes, for example.The IMU 280 and/or INS 270 may be calibrated using the blendednavigation data 290.

For pedestrian navigation, PDR in the place of INS 270 may be integratedwith GNSS receiver. To convert the PDR data to the velocity sampled atthe GNSS sample instances, a method for PDR data conversion for ease ofGNSS/PDR integration is described below. The PDR position data isconverted to user velocity measured at the time instances where GNSSposition/velocity estimates are available. With this method, GNSS/PDRintegration can be implemented at minimum complexity. Most othersolutions do not include PDR data conversion; therefore, the GNSS/PDRblending filter is complicated since it should run at step events (whichis generally irregular), or step events+GNSS position updates (usually 1Hz).

GNSS/IMU Integration Filter with Calibration Features

The GNSS/IMU integration filters in accordance with embodiments of theinvention are based on extended Kalman filter (EKF). Although the EKFsdiscussed here have 10 states, all the GNSS/IMU integration methods thatwill be proposed in this invention can be also applied to

-   -   Any other EKF structures if the state includes the user        velocity, and    -   EKFs in which the state is defined in a coordinate system other        than ECEF (earth-centered earth-fixed). For example, the        position state elements could be in latitude, longitude, and        altitude.

The state of the GNSS/IMU integration EKF is defined in (1). Note thatthe EKF state includes two states, b_(s) and b_(ψ), for DR biasesestimation . . .

x=[x,y,z,−ct _(u) ,{dot over (x)},{dot over (y)},ż,−c{dot over (t)} _(u),b _(s) ,b _(ψ)]^(T).  (1)

where [x, y, z] and [{dot over (x)}, {dot over (y)}, ż] are3-dimensional user position and velocity, respectively, inEarth-Centered, Earth-Fixed (ECEF) coordinate system; t_(u) and {dotover (t)}_(u) represents the clock bias and the clock drift; c is thespeed of light; and b_(s) and b_(ψ) are the state variables for speedbias and the heading bias, respectively, both for the DR measurement.The two biases are further defined as follows:

b _(s) =s _(D) −s  (2)

b _(ψ)=ψ_(D)−ψ  (3)

where s_(D) and ψ_(D) are the speed and the heading from the DRmeasurement, respectively; and s and ψ are the true speed and the trueheading, respectively.

System Equation:

$\begin{matrix}{{x_{k} = {{Ax}_{k - 1} + w_{k}}},{A = \begin{bmatrix}1 & 0 & 0 & 0 & T & 0 & 0 & 0 & 0 & 0 \\0 & 1 & 0 & 0 & 0 & T & 0 & 0 & 0 & 0 \\0 & 0 & 1 & 0 & 0 & 0 & T & 0 & 0 & 0 \\0 & 0 & 0 & 1 & 0 & 0 & 0 & T & 0 & 0 \\0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 \\0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 \\0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 \\0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & ^{{- \beta_{bs}}T} & 0 \\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & ^{{- \beta_{b\; \psi}}T}\end{bmatrix}}} & (4)\end{matrix}$

where T is the sample time (i.e., time difference between two successivestate vectors x_(k-1) and x_(k)); and w_(k) models the process noise.Here, it is assumed that the speed bias and the heading bias arestatistically-independent Gauss-Markov processes defined with parametersβ_(bs) and β_(bψ), respectively. It should be noted that theGauss-Markov assumption is just an example, and other random processesmay be also applied to the integration filters.

Measurement Equation:

The measurement equation for the blending filter is constructed byintegrating the following three types of measurements: i) GNSSpseudorange measurement (per satellite), ii) GNSS delta rangemeasurement (per satellite), and iii) user velocity-related measurementfrom sensor-based DR measurement. The GNSS portion of the measurementequation (pseudorange and delta range measurement) is the same as in theconventional stand-alone GNSS receivers. In order to integrate the DRmeasurements, for each option/embodiment disclosed, differentmeasurement equation (s) are added to the GNSS-related measurementequation.

The measurement equation (generally nonlinear) for the GNSS/IMUintegration filter can be expressed as follows (the time index ‘k’ isdropped for notational simplicity):

$\begin{matrix}{\left\lbrack \frac{z^{G}}{z^{D}} \right\rbrack = {\left\lbrack \frac{h^{G}(x)}{h^{D}(x)} \right\rbrack + \left\lbrack \frac{v^{G}}{v^{D}} \right\rbrack}} & (5)\end{matrix}$

where z^(G) and z^(D) represent the GNSS measurement (pseudorange anddelta range from each satellite) and the DR measurement, respectively.h^(G)(x) is a column vector whose elements are nonlinear functions ofstate x, each function modeling the corresponding GNSS measurement as afunction of x. Similarly, h^(D)(x) is a stack of nonlinear functions ofstate x, each function modeling the corresponding DR measurement as afunction of x. And v^(G) and v^(D) model the measurement noises for theGNSS and the DR measurement, respectively. The measurement equation (5)will be denoted in short as follows:

$\begin{matrix}{{z = {{h(x)} + v}}{where}{{z = \left\lbrack \frac{z^{G}}{z^{D}} \right\rbrack},{{h(x)} = \left\lbrack \frac{h^{G}(x)}{h^{D}(x)} \right\rbrack},{v = {\left\lbrack \frac{v^{G}}{v^{D}} \right\rbrack.}}}} & (6)\end{matrix}$

Since the measurement equations include nonlinear equations, the statecan be estimated using an extended Kalman filter (EKF). In the EKFframework, a linearized measurement model is also used. Combining theGNSS-related measurement and the DR measurement, arrives at thelinearlized measurement equation of form

z=Hx+v  (7)

where the linearized measurement matrix H has two portions each for GNSSand DR, i.e.,

$H = {\left\lbrack \frac{H^{G}}{H^{D}} \right\rbrack.}$

GNSS Portion of Measurement Equation

As mentioned before, the GNSS portion of measurement equation is commonfor all the integration options which are disclosed. Therefore, theGNSS-related measurement equation is written in from (5) as follows:

z ^(G) =h ^(G)(x)+v ^(G)  (8)

The dimension of z^(G) changes depending on the number of satellitemeasurements that are combined in the blending filter. For eachsatellite, two measurements may contribute to the measurement equation(8): pseudorange measurement and delta range measurement. That is, thetwo measurements are populated in vector z^(G) in (8), and the twocorresponding elements of h^(G)(x) model the pseudorange and the deltarange as functions of the current state x with a known satelliteposition. Although not discussed here, other measurements are alsopossible, but do not change the overall measurement equation. Forexample, altitude constraints may be introduced as measurementequations.

In the EKF framework, the nonlinear measurement model (8) is linearizedto have

z ^(G) =H ^(G) x+v ^(G)  (9)

Each row of the measurement matrix H^(G) is determined in terms ofdirection cosines of the unit vector pointing from the user position tothe satellite. For example, when the number of satellites is four, themeasurement equation is as follows:

$\begin{matrix}{\begin{bmatrix}\rho_{1} \\\rho_{2} \\\rho_{3} \\\rho_{4} \\{\overset{.}{\rho}}_{1} \\{\overset{.}{\rho}}_{2} \\{\overset{.}{\rho}}_{3} \\{\overset{.}{\rho}}_{4}\end{bmatrix} = {{\begin{bmatrix}a_{x\; 1} & a_{y\; 1} & a_{z\; 1} & 1 & 0 & 0 & 0 & 0 & 0 & 0 \\a_{x\; 2} & a_{y\; 2} & a_{\; {z\; 2}} & 1 & 0 & 0 & 0 & 0 & 0 & 0 \\a_{x\; 3} & a_{y\; 3} & a_{z\; 3} & 1 & 0 & 0 & 0 & 0 & 0 & 0 \\a_{x\; 4} & a_{y\; 4} & a_{z\; 4} & 1 & 0 & 0 & 0 & 0 & 0 & 0 \\0 & 0 & 0 & 0 & a_{x\; 1} & a_{y\; 1} & a_{z\; 1} & 1 & 0 & 0 \\0 & 0 & 0 & 0 & a_{x\; 2} & a_{y\; 2} & a_{z\; 2} & 1 & 0 & 0 \\0 & 0 & 0 & 0 & a_{x\; 3} & a_{y\; 3} & a_{z\; 3} & 1 & 0 & 0 \\0 & 0 & 0 & 0 & a_{x\; 4} & a_{y\; 4} & a_{z\; 4} & 1 & 0 & 0\end{bmatrix}\begin{bmatrix}x \\y \\z \\{- {ct}_{u}} \\\overset{.}{x} \\\overset{.}{y} \\\overset{.}{z} \\{{- c}{\overset{.}{t}}_{u}} \\b_{s} \\b_{\psi}\end{bmatrix}} + \begin{bmatrix}v_{\rho_{1}} \\v_{\rho_{2}} \\v_{\rho_{3}} \\v_{\rho_{4}} \\v_{{\overset{.}{\rho}}_{1}} \\v_{{\overset{.}{\rho}}_{2}} \\v_{{\overset{.}{\rho}}_{3}} \\v_{{\overset{.}{\rho}}_{4\;}}\end{bmatrix}}} & (10)\end{matrix}$

where ρ_(i) and {dot over (ρ)}_(i) are the pseudorange and the deltarange measurements, respectively, for the i-th satellite. a_(xi),a_(yi), a_(zi) are the x, y, z components of the unit norm vectorpointing from the user position to the i-th satellite.

In practice, there may be any number of measurements and each satellitemay give only a pseudorange or only a delta range measurement. Sometimesthere may not be any available measurements temporarily, in which casethe predicted state is simply the propagation of the last state. In thisdescription, it is assumed that the measurement engine always providestwo measurements for each satellite, but the integration filter maydiscard some measurements. However, the proposed solution can apply toother systems where the measurement does not always provide measurementsin this way as well.

GNSS Portion of Measurement Equation

The integration filter processes the DR measurement, i.e., the uservelocity data from the INS, in the measurement equation of EKF in one ofthe following ways:

-   -   (Option A) Three INS measurements in local navigation coordinate        (e.g., in north, east, down) may be included in the measurement        equation in a way the INS measurements are a function of        velocity variables of an integration filter state with a        plurality of measurement noises.    -   (Option B) Two INS measurements in local navigation coordinate        (e.g., in north, east) may be included in the measurement        equation in a way the INS measurements are a function of        velocity variables of an integration filter state with a        plurality of measurement noises.    -   (Option C) Same as Option A where three INS measurements in        local navigation coordinate (e.g., in north, east, down) may be        included in the measurement equation in a way the INS        measurements are a function of velocity variables of an        integration filter state with a plurality of measurement noises,        but the vertical INS measurement may be set to zero.    -   (Option D) The INS user velocity may be included in the        measurement equation in the form of speed (magnitude of the INS        velocity vector on horizontal navigation plane) and heading        (angle of the INS velocity vector on horizontal navigation        plane).    -   (Option E) Similar to option D, where the INS user velocity may        be included in the measurement equation in the form of speed and        heading; but only the INS user speed (magnitude of the INS        velocity vector on horizontal navigation plane) included in the        measurement equation.    -   (Option F) Similar to option D, where the INS user velocity may        be included in the measurement equation in the form of speed and        heading; but only the INS user heading (angle of the INS        velocity vector on horizontal navigation plane) is included in        the measurement equation.

In one embodiment of the invention, the INS/PDR provides 3-dimensionalnavigation information in local navigation frame, i.e., in NED (north,east, and down). The DR portion of the measurement equation in (5), thatis, z^(D)=h^(D)(x)+v^(D) is given as follows:

$\begin{matrix}{{{\overset{.}{n}}_{D} = {{\left( {1 + \frac{b_{s}}{\sqrt{{\overset{.}{n}}^{2} + {\overset{.}{e}}^{2}}}} \right)\left( {{\overset{.}{n}\; \cos \; b_{\psi}} - {\overset{.}{e}\; \sin \; b_{\psi \;}}} \right)} + v_{{\overset{.}{n}}_{D}}}},} & (11) \\{{{\overset{.}{e}}_{D} = {{\left( {1 + \frac{b_{s}}{\sqrt{{\overset{.}{n}}^{2} + {\overset{.}{e}}^{2}}}} \right)\left( {{\overset{.}{n}\; \sin \; b_{\psi}} + {\overset{.}{e}\; \cos \; b_{\psi}}} \right)} + v_{{\overset{.}{e}}_{D}}}},} & (12) \\{{{\overset{.}{d}}_{D} = {\overset{.}{d} + v_{{\overset{.}{d}}_{D}}}},} & (13)\end{matrix}$

where {dot over (n)}_(D), ė_(D), and {dot over (d)}_(D) are north, east,down component, respectively, of the user velocity that is given by orobtained from the DR measurement. And

$\begin{matrix}{\begin{bmatrix}\overset{.}{n} \\\overset{.}{e} \\\overset{.}{d}\end{bmatrix} = {C_{e,{3 \times 3}}^{n}\begin{bmatrix}\overset{.}{x} \\\overset{.}{y} \\\overset{.}{z}\end{bmatrix}}} & (14)\end{matrix}$

where C_(e,3×3) ^(n) is a 3×3 coordinate transformation matrix (fromECEF to local navigation frame) that is given as follows:

$\begin{matrix}{C_{e,{3 \times 3}}^{n} = {\begin{bmatrix}{{- \sin}\; (\varphi){\cos (\lambda)}} & {{- {\sin (\varphi)}}{\sin (\lambda)}} & {\cos (\varphi)} \\{- {\sin (\lambda)}} & {\cos (\lambda)} & 0 \\{{- {\cos (\varphi)}}{\cos (\lambda)}} & {{- {\cos (\varphi)}}{\sin (\lambda)}} & {- {\sin (\varphi)}}\end{bmatrix}.}} & (15)\end{matrix}$

where φ and λ are the latitude and the longitude of the user position(this can be obtained directly from the previous position estimate).[ν_(n), ν_(e), ν_(d)]^(T) models the measurement noise (zero-meanGaussian random variances with certain variances is assumed).

Since the measurement equations include nonlinear equations, the statecan be estimated using an extended Kalman filter (EKF). The DR portionof the linearized measurement matrix in (7) is given as follows:

$\begin{matrix}{H^{D} = \begin{bmatrix}0 & 0 & 0 & 0 & H_{1,5}^{D} & H_{1,6}^{D} & H_{1,7}^{D} & 0 & H_{1,9}^{D} & H_{1,10}^{D} \\0 & 0 & 0 & 0 & H_{2,5}^{D} & H_{2,6}^{D} & H_{2,7}^{D} & 0 & H_{2,9}^{D} & H_{2,10}^{D} \\0 & 0 & 0 & 0 & c_{3,1} & c_{3,2} & c_{3,3} & 0 & 0 & 0\end{bmatrix}} & (16)\end{matrix}$

where c_(i,j) is the (i,j) component of C_(e,3×3) ^(n) given in (15);and

${H_{1,5}^{D} = {{c_{1,1}\frac{\partial{\overset{.}{n}}_{D}}{\partial\overset{.}{n}}} + {c_{2,1}\frac{\partial{\overset{.}{n}}_{D}}{\partial\overset{.}{e}}}}},{H_{1,6}^{D} = {{c_{1,2}\frac{\partial{\overset{.}{n}}_{D}}{\partial\overset{.}{n}}} + {c_{2,2}\frac{\partial{\overset{.}{n}}_{D}}{\partial\overset{.}{e}}}}},{H_{1,7}^{D} = {{c_{1,3}\frac{\partial{\overset{.}{n}}_{D}}{\partial\overset{.}{n}}} + {c_{2,3}\frac{\partial{\overset{.}{n}}_{D}}{\partial\overset{.}{e}}}}},{H_{1,9}^{D} = {\frac{1}{\sqrt{{\overset{.}{n}}^{2} + {\overset{.}{e}}^{2}}}\left( {{\overset{.}{n}\; \cos \; b_{\psi}} - {\overset{.}{e}\; \sin \; b_{\psi}}} \right)}},{H_{1,10}^{D} = {{- \left( {1 + \frac{b_{s}}{\sqrt{{\overset{.}{n}}^{2} + {\overset{.}{e}}^{2}}}} \right)}\left( {{\overset{.}{n}\; \sin \; b_{\psi}} + {\overset{.}{e}\; \cos \; b_{\psi}}} \right)}},{H_{2,5}^{D} = {{c_{1,1}\frac{\partial{\overset{.}{e}}_{D}}{\partial\overset{.}{n}}} + {c_{2,1}\frac{\partial{\overset{.}{e}}_{D}}{\partial\overset{.}{e}}}}},{H_{2,6}^{D} = {{c_{1,2}\frac{\partial{\overset{.}{e}}_{D}}{{\partial\overset{.}{n}}\;}} + {c_{2,2}\frac{\partial{\overset{.}{e}}_{D}}{\partial\overset{.}{e}}}}},{H_{2,7}^{D} = {{c_{1,3}\frac{\partial{\overset{.}{e}}_{D}}{\partial\overset{.}{n}}} + {c_{2,3}\frac{\partial{\overset{.}{e}}_{D}}{\partial\overset{.}{e}}}}},{H_{2,9}^{D} = {\frac{1}{\sqrt{{\overset{.}{n}}^{2} + {\overset{.}{e}}^{2}}}\left( {{\overset{.}{n}\; \sin \; b_{\psi}} + {\overset{.}{e}\; \cos \; b_{\psi}}} \right)}},{H_{2,10}^{D} = {\left( {1 + \frac{b_{s}}{\sqrt{{\overset{.}{n}}^{2} + {\overset{.}{e}}^{2}}}} \right)\left( {{\overset{.}{n}\; \cos \; b_{\psi}} - {\overset{.}{e}\; \sin \; b_{\psi}}} \right)}},{and}$${\frac{\partial{\overset{.}{n}}_{D}}{\partial\overset{.}{n}} = {{{- \frac{b_{s}\overset{.}{n}}{\left( {{\overset{.}{n}}^{2} + {\overset{.}{e}}^{2}} \right)^{3/2}}}\left( {{\overset{.}{n}\; \cos \; b_{\psi}} - {\overset{.}{e}\; \sin \; b_{\psi}}} \right)} + {\left( {1 + \frac{b_{s}}{\sqrt{{\overset{.}{n}}^{2} + {\overset{.}{e}}^{2}}}} \right)\cos \; b_{\psi}}}},{\frac{\partial{\overset{.}{n}}_{D}}{\partial\overset{.}{e}} = {{{- \frac{b_{s}e}{\left( {{\overset{.}{n}}^{2} + {\overset{.}{e}}^{2}} \right)^{3/2}}}\left( {{\overset{.}{n}\; \cos \; b_{\psi}} - {e\; \sin \; b_{\psi}}} \right)} - {\left( {1 + \frac{b_{s}}{\sqrt{{\overset{.}{n}}^{2} + {\overset{.}{e}}^{2}}}} \right)\sin \; b_{\psi}}}},{\frac{\partial{\overset{.}{e}}_{D}}{\partial\overset{.}{n}} = {{{- \frac{b_{s}\overset{.}{n}}{\left( {{\overset{.}{n}}^{2} + {\overset{.}{e}}^{2}} \right)^{3/2}}}\left( {{\overset{.}{n}\; \sin \; b_{\psi}} + {\overset{.}{e}\; \cos \; b_{\psi}}} \right)} + {\left( {1 + \frac{b_{s}}{\sqrt{{\overset{.}{n}}^{2} + {\overset{.}{e}}^{2}}}} \right)\sin \; b_{\psi}}}},{\frac{\partial{\overset{.}{e}}_{D}}{\partial\overset{.}{e}} = {{{- \frac{b_{s}\overset{.}{e}}{\left( {{\overset{.}{n}}^{2} + {\overset{.}{e}}^{2}} \right)^{3/2}}}\left( {{\overset{.}{n}\; \sin \; b_{\psi}} + {\overset{.}{e}\; \cos \; b_{\psi}}} \right)} + {\left( {1 + \frac{b_{s}}{\sqrt{{\overset{.}{n}}^{2} + {\overset{.}{e}}^{2}}}} \right)\cos \; {b_{\psi}.}}}}$

Even though not explicitly specified, all the variables ({dot over (n)},ė, b_(S), b_(ψ)) in (21) are a priori estimates, (i.e., elements of{circumflex over (x)}_(k) ⁻=A{circumflex over (x)}_(k-1) ⁺).

A second embodiment considers the cases where considers the cases wherethe INS/PDR provides 2-dimensional navigation information in localhorizontal plane (in North and East), or only the 2-dimensionalinformation is reliable even though 3-dimensional navigation is providedby the INS/PDR. The DR portion of the measurement equation in (5), thatis, z^(D)=h^(D)(x)+v^(D) is given as follows:

$\begin{matrix}{{{\overset{.}{n}}_{D} = {{\left( {1 + \frac{b_{s}}{\sqrt{{\overset{.}{n}}^{2} + {\overset{.}{e}}^{2}}}} \right)\left( {{\overset{.}{n}\; \cos \; b_{\psi}} - {\overset{.}{e}\; \sin \; b_{\psi}}} \right)} + v_{{\overset{.}{n}}_{D}}}},} & (17) \\{{{\overset{.}{e}}_{D} = {{\left( {1 + \frac{b_{s}}{\sqrt{{\overset{.}{n}}^{2} + {\overset{.}{e}}^{2}}}} \right)\left( {{\overset{.}{n}\; \sin \; b_{\psi}} + {\overset{.}{e}\; \cos \; b_{\psi}}} \right)} + v_{{\overset{.}{e}}_{D}}}},} & (18)\end{matrix}$

where {dot over (n)}_(D) and ė_(D) are north and east component,respectively, of the user velocity that is given by or obtained from theDR measurement. And

$\begin{matrix}{\begin{bmatrix}\overset{.}{n} \\\overset{.}{e}\end{bmatrix} = {C_{e,{2 \times 3}}^{n}\begin{bmatrix}\overset{.}{x} \\\overset{.}{y} \\\overset{.}{z}\end{bmatrix}}} & (19)\end{matrix}$

where C_(e,2×3) ^(n) is a 2×3 coordinate transformation matrix (fromECEF to local navigation frame) that is given as follows:

$\begin{matrix}{C_{e,{2 \times 3}}^{n} = {\begin{bmatrix}{{- {\sin (\varphi)}}{\cos (\lambda)}} & {{- {\sin (\varphi)}}{\sin (\lambda)}} & {\cos (\varphi)} \\{- {\sin (\lambda)}} & {\cos (\lambda)} & 0\end{bmatrix}.}} & (20)\end{matrix}$

where φ and λ are the latitude and the longitude of the user position(this can be obtained directly from the previous position estimate).[ν_(n),ν_(e)]^(T) models the measurement noise.

Since the measurement equations include nonlinear equations, the statecan be estimated using an extended Kalman filter (EKF). The DR portionof the linearized measurement matrix in (7) is given as follows:

$\begin{matrix}{H^{D} = \begin{bmatrix}0 & 0 & 0 & 0 & H_{1,5}^{D} & H_{1,6}^{D} & H_{1,7}^{D} & 0 & H_{1,9}^{D} & H_{1,10}^{D} \\0 & 0 & 0 & 0 & H_{2,5}^{D} & H_{2,6}^{D} & H_{2,7}^{D} & 0 & H_{2,9}^{D} & H_{2,10}^{D}\end{bmatrix}} & (21)\end{matrix}$

where all the elements of H^(D) (H_(i,j) ^(D):i=1, 2; j=5, 6, 7, 9, 10)are the same as those given in (16).

Another embodiment is the same as the second embodiment above exceptwith one more constraint that the vertical component of the uservelocity is zero.

An additional embodiment considers cases where the DR measurement isgiven in terms of speed and heading. Also, sometimes it is useful to theDR measurement into speed and heading. In this case, The DR portion ofthe measurement equation in (5), that is, z^(p)=h^(D)(x)+v^(D) is givenas follows:

$\begin{matrix}{{s_{D} = {\sqrt{{\overset{.}{n}}^{2} + {\overset{.}{e}}^{2}} + b_{s} + v_{s_{D}}}},} & (22) \\{{\psi_{D} = {{{atan}\; 2\left( \frac{\overset{.}{e}}{\overset{.}{n}} \right)} + b_{\psi} + v_{\psi_{D}}}},} & (23)\end{matrix}$

-   -   where s_(D) and ψ_(D) are the speed and the heading measurement        from INS/PDR, respectively; ν_(s) _(D) and ν_(ψ) _(D) model the        measurement noise. And

$\begin{matrix}{\begin{bmatrix}\overset{.}{n} \\\overset{.}{e}\end{bmatrix} = {C_{e,{2 \times 3}}^{n}\begin{bmatrix}\overset{.}{x} \\\overset{.}{y} \\\overset{.}{z}\end{bmatrix}}} & (24)\end{matrix}$

where C_(e,2×3) ^(n) is given in (20).

Since the measurement equations include nonlinear equations, the statecan be estimated using an extended Kalman filter (EKF). The DR portionof the linearized measurement matrix in (7) is given as follows:

$\begin{matrix}{{H^{D} = \begin{bmatrix}0 & 0 & 0 & 0 & H_{1,5}^{D} & H_{1,6}^{D} & H_{1,7}^{D} & 0 & 1 & 0 \\0 & 0 & 0 & 0 & H_{2,5}^{D} & H_{2,6}^{D} & H_{2,7}^{D} & 0 & 0 & 1\end{bmatrix}}{where}{{H_{1,5}^{D} = {{c_{1,1}\frac{\overset{.}{n}}{\sqrt{{\overset{.}{n}}^{2} + {\overset{.}{e}}^{2}}}} + {c_{2,1}\frac{\overset{.}{e}}{\sqrt{{\overset{.}{n}}^{2} + {\overset{.}{e}}^{2\;}}}}}},{H_{1,6}^{D} = {{c_{1,2}\frac{\overset{.}{n}}{\sqrt{{\overset{.}{n}}^{2} + {\overset{.}{e}}^{2\;}}}} + {c_{2,2}\frac{\overset{.}{e}}{\sqrt{{\overset{.}{n}}^{2} + {\overset{.}{e}}^{2}}}}}},{H_{1,7}^{D} = {{c_{1,3}\frac{\overset{.}{n}}{\sqrt{{\overset{.}{n}}^{2} + {\overset{.}{e}}^{2}}}} + {c_{2,3}\frac{\overset{.}{e}}{\sqrt{{\overset{.}{n}}^{2} + {\overset{.}{e}}^{2}}}}}},{H_{2,5}^{D} = {{{- c_{1,1}}\frac{\overset{.}{e}}{{\overset{.}{n}}^{2} + {\overset{.}{e}}^{2}}} + {c_{2,1}\frac{\overset{.}{n}}{{\overset{.}{n}}^{2} + {\overset{.}{e}}^{2}}}}},{H_{2,6}^{D} = {{{- c_{1,2}}\frac{{\overset{.}{e}}_{u}}{{\overset{.}{n}}^{2} + {\overset{.}{e}}^{2}}} + {c_{{2,2}\;}\frac{\overset{.}{n}}{{\overset{.}{n}}^{2} + {\overset{.}{e}}^{2}}}}},{H_{2,7}^{D} = {{{- c_{1,3}}\frac{\overset{.}{e}}{{\overset{.}{n}}^{2} + {\overset{.}{e}}^{2}}} + {c_{2,3}\frac{\overset{.}{n}}{{\overset{.}{n}}^{2} + {\overset{.}{e}}^{2}}}}},}} & (25)\end{matrix}$

Even though not explicitly specified, all the variables ({dot over (n)},ė, b_(S), b_(ψ)) in (25) are a priori estimates, (i.e., elements of{circumflex over (x)}_(k) ⁻=A{circumflex over (x)}_(k-1) ⁺). Then, thestandard extended Kalman filtering equations summarized in (29) arefollowed.

This embodiment (speed and heading as DR measurements) allows us tocharacterize the speed and heading separately. In some sensorconfigurations, it could have more sense to use different qualitymetrics for speed and heading measurements. For example, inaccelerometer+e-compass configuration, the speed estimation (usingaccelerometer) could be more reliable than the heading estimate frome-compass (since e-compass heading suffers from local magneticdisturbance).

If the DR measurement (speed and heading as DR measurements) has “down”(or vertical) component of user velocity. This embodiment can beextended by adding one more measurement equation:

{dot over (d)} _(D) ={dot over (d)}+ν _({dot over (d)}) _(D)   (26)

Corresponding EKF equation is easily obtained as in Option A.

An additional embodiment, only the speed measurement given in (22) isadded to the DR portion of the measurement equation (5). And thecorresponding linearized measurement matrix consists of the first row ofmatrix given in (25). This option works also for accelerometer-onlyconfiguration. For this embodiment, the integration EKF can have only 9states:

x=[x,y,z,−ct _(u) ,{dot over (x)},{dot over (y)},ż,−ct _(u) ,b_(s)]^(T).  (27)

For yet an additional embodiment, only the heading measurement given in(23) is added to the DR portion of the measurement equation in (5). Andthe corresponding linearized measurement matrix consists of the secondrow of matrix given in (25). For this embodiment, the integration EKFcan have only 9 states:

x=[x,y,z,−ct _(u) ,{dot over (x)},{dot over (y)},ż,−c{dot over (t)} _(u),b _(ψ)]^(T).  (28)

For all the embodiments described above, the standard extended Kalmanfiltering equations are followed, which is summarized below:

{circumflex over (x)} _(k) ⁻ =A{circumflex over (x)} _(k-1) ⁺

P _(k) ⁻ =AP _(k-1) ⁻ A ^(T) +Q _(k)

K _(k) =P _(k) ⁻ H _(k) ^(T)(H _(k) P _(k) ⁻ H _(k) ^(T) +R _(k))⁻¹

{circumflex over (x)} _(k) ⁺ ={circumflex over (x)} _(k) ⁻ +K _(k) [z_(k) −h _(k)({circumflex over (x)} _(k) ⁻)]

P _(k) ⁺=(I−K _(k) H _(k))P _(k) ⁻  (29)

-   -   where Q_(k) is the covariance matrix for the process noise,        i.e., w_(k)□N(0,Q_(k)) (which means Gaussian random vector with        zero-mean and covariance Q_(k)), and R_(k) is the covariance        matrix for the measurement noise, i.e., v_(k)□N(0, R_(k)).

A method of calibration in accordance with an embodiment of theinvention is shown in FIG. 3. Method 300 begins with the creation of acoordinate transformation matrix using the latest position fix (i.e.latitude and longitude) 310. At 320, state variables (for user velocity)to the local navigation coordinate is transformed using said coordinatetransformation matrix. Methods in accordance with embodiments of theinvention can take four paths or embodiments 345, 334, 335, and 333.

Path 345 comprises including a speed measurement in the measurementequation using a magnitude of an INS velocity 340 and including aheading measurement in the measurement equation using an angle of an INSvelocity vector on horizontal navigation plane are included 350A.

Path 334 comprises including only a speed measurement in the measurementequation using a magnitude of an INS velocity 340.

Path 335 comprises including only a heading measurement in themeasurement equation using an angle of an INS velocity vector onhorizontal navigation plane are included 350B.

Path 333 comprises a plurality of INS measurements in a local navigationcoordinate is included in said measurement equation in a way that saidINS measurements are a function of velocity variables and speed and/orheading bias variables of an integration filter state with a pluralityof measurement noises 330.

Paths 345, 334, 335, and 333 each also comprise 360 and 370. At 360, thestate variables of said integration filter are estimated. The statevariables include speed bias and/or heading bias. For example, if Kalmanfilter used by processing a set give in (29)

{circumflex over (x)} _(k) ⁻ =A{circumflex over (x)} _(k-1) ⁺

P _(k) ⁻ =AP _(k-1) ⁻ A ^(T) +Q _(k)

K _(k) =P _(k) ⁻ H _(k) ^(T)(H _(k) P _(k) ⁻ H _(k) ^(T) +R _(k))⁻¹

{circumflex over (x)} _(k) ⁺ ={circumflex over (x)} _(k) ⁻ +K _(k) [z_(k) −h _(k)({circumflex over (x)} _(k) ⁻)]

P _(k) ⁺=(I−K _(k) H _(k))P _(k) ⁻;

A blended calibrated position fix is outputted at 370.

Option for GNSS Outage

When the GNSS measurement is unreliable (e.g., in GNSS outage, forexample, indoor), tracking the DR biases is not very meaningful, sincethere is no reference that can be used for estimating the DR biases. Insuch cases, one good way is to freeze the state variables for the DRbiases at GNSS outage, or more generically, when the quality of the GNSSmeasurement is not good. Among others, the following is a simple way toimplement: To set the process noise variances for the DR bias states tosmall numbers. The time instance for changing the process noisevariances for the DR bias states is determined based on the quality ofthe GNSS measurement. For example, reduce the process noise varianceswhen GNSS position uncertainty is larger than a threshold (in GNSSoutage).

Balancing Between GNSS and IMU

Blending filter is so flexible that the following options are allowed:

Selective IMU Integration: The integration filter is configured as astand-alone GNSS position engine and does not integrate said INSmeasurements when GNSS measurements are reliable (when GNSS signalcondition is good). For the GNSS signal condition, any metric for GNSSsignal quality can be used. One example could be the number ofsatellites whose signal level with respect to noise level is greaterthan a threshold (number of available satellites). The other example isGNSS position or velocity uncertainty metric.

Continuous GNSS/IMU Integration: The measurement noise variances (perSV) for GNSS measurement are determined based on the signal quality. Themeasurement noise is time-varying and location-dependent. For example,it is high in bad signal condition (e.g., blockage, multipath). Themeasurement noise variance for INS user velocity data could bedetermined based on the followings: accuracy of sensors, mountingcondition of IMU, and dynamics of the receiver, etc. Or, the measurementnoise variance for INS user velocity may be set to be a constant (notchanging over time). By these, the integration filter (EKF) balancesbetween GNSS and IMU by itself. That is, more weight on IMU when GNSSsignal is not good.

Many modifications and other embodiments of the invention will come tomind to one skilled in the art to which this invention pertains havingthe benefit of the teachings presented in the foregoing descriptions,and the associated drawings. Therefore, it is to be understood that theinvention is not to be limited to the specific embodiments disclosed.Although specific terms are employed herein, they are used in a genericand descriptive sense only and not for purposes of limitation.

What is claimed is: 1-26. (canceled)
 27. A method of calibration comprising: creating a coordinate transformation matrix using a latest position fix (latitude and longitude); transforming said state variables (for user velocity) to a local navigation coordinate using said coordinate transformation matrix; including a plurality of INS measurements in said local navigation coordinate in a measurement equation in a way that said INS measurements are a function of a plurality of velocity variables and speed and/or heading bias variables of an integration filter state with a plurality of measurement noises; estimating said state variables (which include speed bias and/or heading bias) of said integration filter. outputting a blended calibrated position fix.
 28. The method of claim 27, wherein said speed bias and/or said heading bias are not updated when the GNSS measurement is unreliable.
 29. The method of claim 27, further comprising setting a plurality of process noise variances for said speed bias and/or said heading bias to small numbers when the quality of the GNSS measurement is not good, so that state variables for speed bias and/or said heading bias are effectively not changing over time.
 30. The method of claim 27, further comprising determining a time instance for changing the process noise variances for the DR bias states based on the quality of the GNSS measurement.
 31. The method of claim 30, wherein said time instance for changing the process noise variances for the DR bias states is determined when GNSS position uncertainty is larger than a threshold during a GNSS outage.
 32. The method of claim 27, wherein determination of said measurement noise variance for INS user velocity is selected from the group consisting of: accuracy of sensors, mounting condition of IMU, and dynamics of the receiver.
 33. A method of calibration comprising: creating a coordinate transformation matrix using a latest position fix (latitude and longitude); transforming said state variables (for user velocity) to a local navigation coordinate using said coordinate transformation matrix; including a heading measurement using an angle of said INS velocity vector on horizontal navigation plane; estimating said state variables (which include speed bias and/or heading bias) of said integration filter. outputting a blended calibrated position fix.
 34. A method of calibration comprising: creating a coordinate transformation matrix using a latest position fix (latitude and longitude); transforming said state variables (for user velocity) to a local navigation coordinate using said coordinate transformation matrix; including a speed measurement using a magnitude of an INS velocity vector on horizontal navigation plane; estimating said state variables (which include speed bias and/or heading bias) of said integration filter. outputting a blended calibrated position fix.
 35. The method of claim 34, further comprising including a heading measurement using an angle of said INS velocity vector on horizontal navigation plane. 